'''
Created on Aug 5, 2019

@author: modys
'''

if __name__ == '__main__':
    pass
from kinematicModel import Bicycle
from dynamicModel import Vehicle
import matplotlib.pyplot as plt
import numpy as np


# sample_time = 0.01
# time_end = 30
# model = Bicycle()
# model.reset()
# 
# t_data = np.arange(0,time_end,sample_time)
# x_data = np.zeros_like(t_data)
# y_data = np.zeros_like(t_data)
# v_data = np.zeros_like(t_data)
# 
# v_data[:] = 4.5 * np.pi * 8 / time_end;
# length = len(t_data)
# quarter_circle = np.int(len(t_data)/9) +4
# circle = np.int(quarter_circle *4)
# inc = quarter_circle + circle;
# 
# model.delta = np.arctan(2/8)
# for i in range(inc):
#     x_data[i] = model.xc
#     y_data[i] = model.yc
#     model.step(v_data[i],0 )
#     
# model.delta = np.arctan(-2/8)
# for i in range((length-inc)):
#     x_data[i+inc] = model.xc
#     y_data[i+inc] = model.yc
#     model.step(v_data[i+inc],0 )
# 
# plt.axis('equal')
# plt.plot(x_data, y_data,label='Learner Model')
# 
# plt.legend()
# plt.grid()
# plt.show()

def(input,m,min_x,max_x,c):
    
sample_time = 0.01
time_end = 20
model = Vehicle()

t_data = np.arange(0,time_end,sample_time)*1
v_data = np.zeros_like(t_data)
throttle = np.zeros_like(t_data)
alpha = np.zeros_like(t_data)
# throttle percentage between 0 and 1
throttle = 0.2

# incline angle (in radians)
alpha = 0

for i in range(t_data.shape[0]):
    v_data[i] = model.v
    model.step(throttle, alpha)
    
plt.plot(t_data, v_data)
plt.show()